
/*****************************************************************************\
 *                Benchmark for collision detection
\*****************************************************************************/

/*! @file
 *
 *  @brief
 *    Benchmark program for collision detection algorithms.
 *
 *  Type './bench -h' for an explanation of the command line options and keys.
 *
 *  Keys: (interactive version)
 *  - l     : lighting on/off
 *  - p     : polygon mode (wireframe, dots, filled)
 *
 * If you are looking for an example of how to use the collision detection
 * API, this is the wrong place! Look at movem.cpp instead.
 *
 *  @author Gabriel Zachmann
 *	- reimplemented by Weiyu Yi, wyi@tu-clausthal.de
 *
 *  @todo
 *  - Loader einbauen
 *  - Zwei Zeitspalten ausgeben: eine fuer Zeiten im Falle der Koll.,
 *    eine fuer den Fall der Nicht-Koll.; dann kann man besser vergleichen, ob
 *    eine Optimierung beide Faelle verbessert, oder nur einen davon.
 *  - Alle Programme, die ein Window anzeigen koennen, sollen
 *    SimpleSceneManager verwenden.
 */



//---------------------------------------------------------------------------
//  Includes
//---------------------------------------------------------------------------

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <algorithm>
#include <list>

#if defined(__APPLE__)
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
#endif

#include <Collision.h>
#include <ColExceptions.h>
#include <ColObj.h>
#include <ColUtils.h>

#include <ColUtilsOpenGL.h>

#ifdef _WIN32
#ifdef max
#undef max
#endif
#ifdef min
#undef min
#endif
#endif

using col::Point3;
using col::ColGeometry;
using col::Primitive;
using col::Cube;
using col::Icosahedron;
using col::Trackball;

using col::Quat;
using col::Vector3;
using col::Vector4;
using col::Matrix4;
using col::Boxtree;
using col::DopTree;

using col::ColID;

using col::operator * ;

/***************************************************************************\
 *                        Global variables                                 *
\***************************************************************************/

namespace {

typedef enum
{
	PERSPECTIVE,
	ORTHOGRAPHIC	
} ProjectionE;

// constants
const Vector3 rot_axis1( 0, 0, 1 );						// same as in Y/bench
const Vector3 rot_axis2( 1, 0, 0 );
const unsigned int MaxNumDistances = 100;

// options
unsigned int		Complexity = 4;
bool				With_window = false;
bool				No_timing = false;
unsigned int		Ndistances = 2;
unsigned int		Nrotations = 10;
double				Rot_vel;
double				Dist_vel;
double				Dist_1 = 0.0;
double				Dist_2 = 1.5;
col::AlgoE			algo = col::ALGO_DOPTREE;
bool				White_background = false;
bool                All_collisions = false;

enum GeometryTypeE
{
	OBJ_PLANES,							// prefix needed just b/c of stupid M$
	OBJ_SPHERES,
	OBJ_CUBES
};
GeometryTypeE geom_type = OBJ_PLANES;

enum VerboseFlagsE
{
	VERBOSE_PRINT		= 0x01
};
int 					verbose = 0;
bool					col_verbose = 0;
bool					col_verboseShowHulls = 0;

// state
unsigned int		ncollisions = 0;
col::MatrixCell		*cell;					// checks a pair of objects
col::Callback		*callback;				// just needed to make cell happy
col::ColObj			*cobj_m, *cobj_f;		// coll. objects for cell

// stats
struct TimingData
{
	double time;
	double dist;
	unsigned int ncols;
};

TimingData timing[MaxNumDistances];
double build_time;

// nodes
ColGeometry* fixed_geom;
ColGeometry* moving_geom;

Matrix4 fixed_matrix;
Matrix4 moving_matrix;

// render options
bool animation_started = false;

GLuint w, h;
ProjectionE proj = PERSPECTIVE;
GLenum fill_mode = GL_LINE;

Trackball tball;
GLdouble view_distance = 3.0;
Matrix4 viewing_matrix;

GLdouble fixed_color[3] = { 0.5, 0.5, 0.7 };
GLdouble moving_color[3] = { 0.7, 0.5, 0.5 };

}


/***************************************************************************\
 *                       Check and  process coll                           *
\***************************************************************************/

void move_and_check( void )
{
	static unsigned int rot_step = 0;
	static unsigned int dist_step = 0;
	static bool first_time = true;
	static double start;
	static unsigned int ncols = 0;
	static unsigned int cycle = 0;
	bool c = false;

	if ( first_time )
	{
		start = col::time();
		first_time = false;
	}

	moving_matrix = Matrix4::rotation( rot_step*Rot_vel, rot_axis1 ) *
				  Matrix4::rotation( rot_step*Rot_vel, rot_axis2 );
	moving_matrix.setTranslation( Vector3( Dist_1 + dist_step * Dist_vel, 0, 0 ) );

	// update toworld matrix stored inside ColObj's
	cycle ++ ;
	cobj_f->hasMoved( cycle );
	cobj_m->hasMoved( cycle );
	// be careful! this matrix must be allocated from heap, not stack
	cobj_m->setTransMatrix( new Matrix4( moving_matrix ) );

	// perform coll. det., without convex hull pre-check
	c = cell->check( false );

	if ( c )
	{
		ncollisions ++ ;
		ncols ++ ;

		if ( verbose & VERBOSE_PRINT )
		{
			putchar('!');
			fflush(stdout);
		}
	}

	rot_step ++ ;
	if ( rot_step >= Nrotations )
	{
		// one distance finished, save data
		double stop;
		stop = col::time();
		timing[dist_step].time = (stop-start) / static_cast<double>(Nrotations);
		timing[dist_step].ncols = ncols;
		timing[dist_step].dist = Dist_1 + dist_step * Dist_vel;

		if ( With_window )
		{
			static bool first_time_here = true;
			if ( first_time_here )
			{
					puts("# dist:  coll. time (msec):   num. colls.:");
					first_time_here = false;
			}
			printf("%3.3f  %10.4f  %6u\n",
				   timing[dist_step].dist, timing[dist_step].time,
				   timing[dist_step].ncols );
		}

		ncols = rot_step = 0;
		dist_step ++ ;

		if ( dist_step >= Ndistances )
			dist_step = 0;

		start = col::time();
	}
}



void benchExit( void )
{
	if ( No_timing )
		puts("# dist:  num. colls.:");
	else
		puts("# dist:  coll. time (msec):   num. colls.:");

	for ( unsigned int i = 0; i < Ndistances; i ++ )
		if ( No_timing )
			printf("%3.3f  %6u\n", timing[i].dist, timing[i].ncols );
		else
			printf("%3.3f  %10.4f  %6u\n",
				   timing[i].dist, timing[i].time, timing[i].ncols );

	printf("# num collisions = %u\n", ncollisions );
	printf("# num faces = %u\n", fixed_geom->getPointsPtr()->size() );
	if ( No_timing  == false )
		printf("# build time = %f (msec)\n", build_time / 2.0 );
}



// dummy class in order to implement operator () so we can create instances
class BenchCallback : public col::Callback
{
public:
    BenchCallback( const ColID obj1, const ColID obj2,
                   col::LevelOfDetectionE level );
    virtual void operator () (const col::Data *data) throw ();
};


BenchCallback::BenchCallback( const ColID id1, const ColID id2,
							  col::LevelOfDetectionE inlevel )
:	col::Callback(id1,id2,false,false,inlevel)
{
	if ( All_collisions )
		all_polygons = true;
}


void BenchCallback::operator () (const col::Data * /*data*/ ) throw ()
{
	puts("bench: BUG: coll. callback has been called!"
		   " (should not happen)");
}



/***************************************************************************\
 *                      Uitility functions                                 *
\***************************************************************************/

/**  Create geometry
 *
 * @param type			the type of geometry to create
 * @param complexity	determines number of polygons
 *
 * The number of polygons is usually something like complexity^2 .
 *
 **/

void createGeom( GeometryTypeE type, unsigned int complexity, ColGeometry *geom )
{
	switch ( type )
	{
		case OBJ_PLANES:
		{
			makePlane( geom, 1, 1, complexity, complexity );
			(*geom->editPointsPtr())[0][2] += 3;
			(*geom->editPointsPtr())[ geom->getPointsPtr()->size()-1 ][2] -= 3;
			geom->updateBBox();

			break;
		}

		case OBJ_SPHERES:
			complexity = static_cast<unsigned int>( log( static_cast<REAL>(complexity*complexity) ) / 1.7 );
			col::makeSphere( geom, complexity, 0.5 );
			break;

		case OBJ_CUBES:
			col::makeCube( geom, 0.5 );
			break;

		default:
			fputs("createGeom: BUG: type is out of range!\n",stderr);
			exit(-1);
	}
}

/***************************************************************************\
 *                        Display and User input                           *
\***************************************************************************/

void init(void) 
{
	glClearColor(0.0, 0.0, 0.0, 0.0);
	glShadeModel(GL_FLAT);
	glEnable(GL_DEPTH_TEST);
}

void reshape( int width, int height )
{
	w = width;
	h = height;
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	tball.setViewport( w, h );
}

void animate( void )
{
	if ( animation_started == false )
		return;

	move_and_check();
	glutPostRedisplay();
}

void display(void)
{
	// trackball 
	if ( tball.getTrackMode() == col::TRACK_ROTATE )
	{
		REAL angle;
		REAL axis[3];
		tball.getRotation( axis, angle );
		Matrix4 m = Matrix4::rotation( angle, col::Vector3( axis[0], axis[1], axis[2] ) );
		viewing_matrix = m * viewing_matrix;
	}
	else
	if ( tball.getTrackMode() == col::TRACK_ZOOM )
	{
		REAL zoom;
		tball.getZoom( zoom );
		Matrix4 m = Matrix4::scale( col::Vector3( zoom+1 ) );
		viewing_matrix = m * viewing_matrix;
	}
	

	// all geometries are ready to render
	// --------------------------------------------------------
	// display begins
	
	// Prjection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity ();

	GLdouble range=20.0;
	switch( proj )
	{
		case ORTHOGRAPHIC:
			// orthographic projection
			if (w <= h)
				glOrtho(-1*range, range, -1*range*(GLdouble)h/(GLdouble)w,
						range*(GLdouble)h/(GLdouble)w, 0.1, 100.0);
			else
				glOrtho(-1*range*(GLdouble)w/(GLdouble)h,
						range*(GLdouble)w/(GLdouble)h, -1*range, range, 0.1, 100.0);
			break;
		case PERSPECTIVE:
			// perspective projection
			gluPerspective(60, (GLdouble)w/(GLdouble)h, 0.1, 100.0);
			break;
		default:
			fprintf( stderr, "No such projection mode!\n" );
			break;
	}

	glMatrixMode( GL_MODELVIEW );
	glLoadIdentity();
	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

	// View Transformation
	glTranslated( 0.0, 0.0, -view_distance );

	GLdouble view_mat[16];
	for ( unsigned int i=0; i<4; i++ )
	for ( unsigned int j=0; j<4; j++ )
		view_mat[ i*4+j ] = viewing_matrix[i][j];
	glMultMatrixd( view_mat );

	// Modelling Transformation and Drawing

	if ( fixed_geom != NULL )
	{
		glPushMatrix();
		GLdouble mat[16];
		for ( unsigned int i=0; i<4; i++ )
			for ( unsigned int j=0; j<4; j++ )
				mat[ i*4+j ] = fixed_matrix[i][j];
		glMultMatrixd( mat );

		col::drawGeometry( fixed_geom, fill_mode, fixed_color );
		glPopMatrix();
	}
	
	if ( moving_geom != NULL )
	{
		glPushMatrix();
		GLdouble mat[16];
		for ( unsigned int i=0; i<4; i++ )
			for ( unsigned int j=0; j<4; j++ )
				mat[ i*4+j ] = moving_matrix[i][j];
		glMultMatrixd( mat );

		col::drawGeometry( moving_geom, fill_mode, moving_color );
		glPopMatrix();
	}

	//
	glutSwapBuffers();
}

void mouseClick( int button, int state, int x, int y )
{
	tball.mouseClick( button, state, x, y );
}

void mouseMoving( int x, int y )
{
	tball.mouseMoving( x, y );
	glutPostRedisplay();
}


void keyboard(unsigned char key, int x, int y)
{
	switch (key) {
		case 27:
			benchExit();
			exit(0);
			break;
		case ' ':
			if ( animation_started == false )
			{
				animation_started = true;
				glutIdleFunc( animate );
			}
			else
			if ( animation_started == true )
			{
				animation_started = false;
				glutIdleFunc( NULL );
			}
			break;
		case 'p':
		case 'P':
			if ( proj == PERSPECTIVE )
				proj = ORTHOGRAPHIC;
			else
			if ( proj == ORTHOGRAPHIC )
				proj = PERSPECTIVE;

			glutPostRedisplay();
			break;
		case 'i':
		case 'I':
			if ( fill_mode == GL_LINE )
				fill_mode = GL_FILL;
			else
			if ( fill_mode == GL_FILL )
				fill_mode = GL_POINT;
			else
			if ( fill_mode == GL_POINT )
				fill_mode = GL_LINE;

			glutPostRedisplay();
			break;
		default:
			break;
	}
}


/***************************************************************************\
 *                              Main                                       *
\***************************************************************************/

void commandlineerror( char *cmd, char *parm )
{
	if ( cmd )
		fprintf(stderr, "Offending option: %s\n", cmd );
	if ( parm )
		fprintf(stderr, "with first parameter: %s\n", parm );

	fprintf(stderr, "\n\nUsage: bench options ...\n"
	"Options:\n"
	"-x compl    complexity (#pgons ~ compl^2)\n"
	"-g obj      geometry type (default = pl)\n"
	"              obj = pl, sh, bx;\n"
	"-n d r      perform d distances and r rotations for each distance\n"
	"              if d=1, then only distance d1 will assumed\n"
	"-d d1 d2    move object from distance d1 to d2 while rotating it\n"
	"-a deg      each rotations is about deg degrees (default = 360/r)\n"
	"              r = rotational resolution (4r3), a = axis resolution\n"
	"-A algo     algo to use for coll. det. (default algo = do)\n"
	"              do = doptree,\n"
	"              bx = boxtree,\n"
	"-T          do not print timing info (for regression tests)\n"
	"-v options  verbose mode, timing will be bogus, options are:\n"
	"              v = set verbose flag in col.det. module\n"
	"              b = print a ! for each collision\n"
	"-w          with window; timing will be bogus\n"
	"            (program won't stop automatically)\n"
	"-W          white background (default = black)\n"
	"-c          compute all collisions (else the programs stops traversal\n"
	"            when it has found the first collision\n"
	"-h          this help menu\n"
	"Keys:\n"
	"l           switch lighting mode\n"
	"p           switch drawing mode (filled/wireframe/point)\n"
	"q           quit\n"
	"\n");

	if ( cmd )
		exit(-1);				// problem occured
	else
		exit(0);				// was option -h
}


void parsecommandline( int argc, char *argv[] )
{
	/* valid option characters; last char MUST be 0 ! */
	char optionchar[] =   { 'h', 'g', 'x', 'v', 'n', 'a', 'w', 'd', 'A',
							'T', 'W', 'c',  '\0' };
	int musthaveparam[] = {  0 ,  1,   1,   1,   2,   1,   0,   2,   1,
							 0,   0,   0,   };
	int nopts;
	int mhp[256];
	int isopt[256];
	int optchar;


	nopts = strlen(optionchar);
	if ( nopts > 50 )
	{
		fprintf(stderr, "\n\nparsecommandline: the option-chars string "
				"seems to be\nVERY long (%d bytes) !\n\n", nopts );
		exit(-1);
	}

	fill_n( isopt, 256, 0 );
	fill_n( mhp, 256, 0 );
	for ( int i = 0; i < nopts; i ++ )
	{
		if ( isopt[static_cast<int>(optionchar[i])] )
		{
			fprintf(stderr, "\n\nparsecommandline: Bug: option character '%c'"
					" is specified twice in the\n"
				   "option character array !\n\n", optionchar[i] );
			exit(-1);
		}
		isopt[ static_cast<int>(optionchar[i]) ] = 1;
		mhp[ static_cast<int>(optionchar[i])] = musthaveparam[i];
	}

	++argv; --argc;
	while ( argc > 0 )
	{
		if ( argv[0][0] == '-' )
		{
			optchar = argv[0][1];

			if ( ! isopt[optchar] )
			{
				fprintf(stderr, "\nIs not a valid command line option\n");
				commandlineerror( argv[0], NULL );
			}
			for ( int i = 0; i < mhp[optchar]; i ++ )
				if ( ! argv[1+i] || argv[1+i][0] == '-' )
				{
					fprintf(stderr, "\nCommand line option -%c must "
							"have %d parameter(s)\n",
							argv[0][1], mhp[optchar] );
					commandlineerror( argv[0], NULL );
					argv += 1 + i;
					argc -= 1 + i;
					continue;
				}

			switch ( optchar )
			{
				case 'h': commandlineerror( NULL, NULL);  break;

				case 'w': With_window = true; break;
				case 'T': No_timing = true; break;
				case 'W': White_background = true; break;
				case 'c': All_collisions = true; break;

				case 'x': Complexity = atoi( argv[1] ); break;
				case 'a': Rot_vel = atof( argv[1] ); break;

				case 'n': Ndistances = atoi( argv[1] );
						  Nrotations = atoi( argv[2] );
						  break;

				case 'd': Dist_1 = atof( argv[1] );
						  Dist_2 = atof( argv[2] );
						  break;


				case 'v': for ( unsigned int i = 0; i < strlen(argv[1]); i ++ )
							  switch ( argv[1][i] )
							  {
								  case 'v': col_verbose = true;
											break;
								  case 'b': verbose |= VERBOSE_PRINT;
											break;
								  case 'h': col_verboseShowHulls = true;
											break;
								  default : commandlineerror(argv[0],argv[1]);
							  }
						  break;

				case 'g': if ( ! strcmp(argv[1],"pl") )
							  geom_type = OBJ_PLANES;
						  else
						  if ( ! strcmp(argv[1],"sh") )
							  geom_type = OBJ_SPHERES;
						  else
						  if ( ! strcmp(argv[1],"bx") )
							  geom_type = OBJ_CUBES;
						  else
						  {
							  fputs("unrecognized obj type\n",stderr);
							  commandlineerror(argv[0],argv[1]);
						  }
						  break;

				case 'A': if ( ! strcmp(argv[1],"do") )
							  algo = col::ALGO_DOPTREE;
						  else
						  if ( ! strcmp(argv[1],"bx") )
							  algo = col::ALGO_BOXTREE;

						   else
						   {
							  fputs("unrecognized algorithm\n",stderr);
							  commandlineerror(argv[0],argv[1]);
						   }
						   break;

				default: fprintf(stderr, "\nBug in parsecommandline !\n");
						 commandlineerror( argv[0], NULL );
			}

			argv += 1 + mhp[optchar];
			argc -= 1 + mhp[optchar];
		}
		else
		{
			/* command line arg doesn't start with '-' */
			fprintf(stderr, "\nThis is not a valid command line option\n");
			commandlineerror( argv[0], NULL );
			/* or, load file instead .. */
		}
	}

	// check sanity of options
	if ( Complexity < 1 ||
		 (geom_type != OBJ_PLANES && Complexity < 3) ||
		 Complexity > 1000 )
	{
		fprintf(stderr,"complexity (%u) out of range!\n", Complexity );
		exit(-1);
	}

	if ( Ndistances > MaxNumDistances )
	{
		fprintf(stderr,"too many (%u) distances!\n", Ndistances );
		Ndistances = MaxNumDistances;
	}
	if ( Ndistances < 1 )
		Ndistances = 1;

	if ( Rot_vel < col::NearZero )
		Rot_vel = 360.0f / Nrotations;
	Rot_vel *= 2.0 * 3.1415926535 / 360.0;

	if ( Ndistances > 1 )
		// Dist_vel will be used like this:
		// d = Dist_1 + dist_step * Dist_vel
		Dist_vel = (Dist_2 - Dist_1) / (Ndistances - 1);
	else
		Dist_vel = 0.0;
}


int main( int argc, char *argv[] )
{
	// parse command line options
	parsecommandline( argc, argv );

	// initializeing data
	viewing_matrix = Matrix4::identity();

	// create fixed object
	fixed_geom = new ColGeometry();
	createGeom( geom_type, Complexity, fixed_geom );
	fixed_matrix = Matrix4::identity();

	// create moving object
	moving_geom = new ColGeometry();
	createGeom( geom_type, Complexity, moving_geom );
	moving_matrix = Matrix4::identity();

	// prepare glut
	glutInit( &argc, argv );
	glutInitDisplayMode ( GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB );
	glutInitWindowSize( 350, 350 ); 
	glutInitWindowPosition( 100, 100 );
	glutCreateWindow( argv[0] );
	init();

	// register glut callback
	glutDisplayFunc( display ); 
	glutReshapeFunc( reshape );
	glutMouseFunc( mouseClick );
	glutMotionFunc( mouseMoving );
	glutKeyboardFunc( keyboard );

	try
	{
		// Collision detection init
        col::CollisionPipeline *pipeline = new col::CollisionPipeline("pipe",1);

        pipeline->verbose(col_verbose,col_verboseShowHulls);

		// register objects with collision detection module
		build_time = col::time();

		// fixed_geom has ColID 0, moving_geom has ColID 1
		cobj_m = new col::ColObj( 1, moving_geom, 
								  false, false, false, algo, NULL, false );
		cobj_f = new col::ColObj( 0, fixed_geom, 
								  false, false, false, algo, NULL, false );
		build_time = col::time() - build_time;

		// create cell for pair, just needed to set level-of-detection
		cell = new col::MatrixCell( cobj_m, cobj_f );

		// todo
		callback = new BenchCallback( 1/*moving_node*/, 0/*fixed_node*/, col::LEVEL_EXACT );

		cell->addCallback( callback );

	}
	catch ( col::XCollision &x )
	{
		fputs("bench: collision exception!\n",stderr);
		x.print();
		exit(-1);				// in a real app, we would try to continue
	}

	// run...
	if ( With_window )
		glutMainLoop();
	else
	{
		for ( unsigned int i = 0; i < Nrotations*Ndistances; i ++ )
			move_and_check();
		benchExit();
	}

	return 0;
}



/**  Zum Abpasten fuer eine Funktion, Methode, oder Makro-"Funktion".
 *
 * @param param1	Comment for param1
 *
 * @return
 *   Text for return value.
 *
 * Detaillierte Beschreibung ...
 *
 * @throw Exception
 *  Beschreibung ..
 *
 * @warning
 *   Dinge, die der Aufrufer unbedingt beachten muss...
 *
 * @assumptions
 *   Annahmnen, die die Funktion macht...
 *
 * @sideeffects
 *   Nebenwirkungen, globale Variablen, die veraendert werden, ..
 *
 * @todo
 *   Was noch getan werden muss
 *
 * @bug
 *   Bekannte Bugs dieser Funktion
 *
 * @see
 *   ...
 *
 * @internal
 *   Implementierungsdetails, TODOs, ...
 *
 **/



